#ifndef CURCONTROL_H
#define CURCONTROL_H
#include <memory>
#include <vector>
#include <functional>
#include <thread>
#include <atomic>
#include <string>


namespace CURCONTROL {
class RTDEControl;
class ScriptClient;
class DashBoardClient;
}

namespace CURCONTROL {

/*!
* \brief UR5对象类：控制，数据通信...(函数命名兼容旧式接口风格)
* \brief 该类包含脚本客户端句柄，dashborad客户端句柄，rtde控制类句柄
*/
class CURControl
{
public:
    enum class DashBoardCommand{
        UNKNOW,
        POWER_ON,
        POWER_OFF,
        SHUTDOWN,
        BRAKE_RELEASE,
        CLOSE_SAFETY_POPUP,
        UNLOCK_PROTECTIVE_STOP,
        RESTART_SAFETY,
        GET_ROBOT_MODE
    };
private:
    /*!
    * \brief 负责机械臂的power on、brake release、shutdown等工作
    */
    std::shared_ptr<DashBoardClient> dashboard_;// dashborad客户端句柄
    /*!
    * \brief 一旦与机械臂建立链接后，负责发送RTDE脚本给UR，发送成功将在UR系统上运行
    * \brief RTDE脚本类似于服务端程序，与客户端通过30004端口进行通信交互
    */
    std::shared_ptr<ScriptClient> script_;// 脚本客户端句柄
    /*!
    * \brief 负责机械臂的RTDE控制指令执行、RTDE数据实时获取与解析等
    */
    std::shared_ptr<RTDEControl> rtde_;// rtde控制类句柄
private:
    std::thread *publishThread_;
    std::atomic_bool running_;
    std::atomic<uint32_t> ctlCycleTimeMs_;// 控制周期时间 单位:ms
    std::atomic_bool enableFreedrive_;// freedrive使能
private:
    std::function<void(bool)> freedriveFunc_;
    std::function<void(int, int, int)> modeChangedFunc_;
    std::function<void(bool conStatus)> connectStatusChangedFunc_;// 连接状态改变回调函数
    std::string robotIp_;
public:
    CURControl();
    ~CURControl();

    void PublishThread();
    int connectToRobot(const std::string &ip);
    inline uint32_t getCtlCycleTimeMs(){
        return this->ctlCycleTimeMs_.load();
    }
    int disconnectToRobot();

    int setEnabled(bool, int type = 0);
    int getError(int*);
    int getStatus(int* = nullptr, int* = nullptr, int* = nullptr);
    int isConnected();
    int getForce(double*);

    int setSpeedLevel(double);
    long long getDigitalInput();
    long long getDigitalOutput();
    int getQdActual(double*);
    int getIActual(double*);
    /*!
   * \brief 获取UR法兰基于base的位姿
   *
   * \param[out] 法兰位姿，姿态是旋转向量(rx,ry,rz)
   *
   * \returns 成功返回0
   */
    int getToolVectorActual(std::vector<double> &pos);
    int getTcpSpeedActual(double*);
    int getMotorTemperatures(double*);
    int getJointModes(double*);
    int getToolAccelerometerValues(double*);
    double getLinearMomentumNorm();
    int getVActual(double*);
    double getIRobot();
    double getVRobot();
    double getVMain();

    double getUrVersion();
    int turnOn(int);
    int turnOff();
    int resetError(int);
    int getQActual(double*);
    void GetActualTcpForce(std::vector<double> &value);
    void GetActualCurrent(std::vector<double> &value);
    void GetActualTemperatures(std::vector<double> &value);
    //    int sendCommand(const char*, int);
    //    int sendControlCommand(const char*, int, std::string*);
    int servoJ(double*, double acc = 1.2, double v = 0.25, double t = 0.008, double lookahead_time = 0.1, int gain = 300);
    int MoveL(std::vector<double> pose, double acc = 1.4, double v = 1.05, double t = 0.008, double r = 0);
    int SpeedL(std::vector<double> pose, double acc = 0.5, double t = 0.05);
    int ServoJPlus(std::vector<double> pose, double acc = 1.5, double v = 1,
                   double t = 0.01, double lookahead_time = 0.1, int gain = 300);
    void SetAccAndTime(double acc, double t);


    void setFreeDriveCallBack(std::function<void(bool)>);
    void setConnectCallBack(std::function<void(bool)>);
    void setStatusChangeCallBack(std::function<void(void*)>);
    void setModeChangeCallBack(std::function<void(int, int, int)>);
    void setToolVoltage(unsigned int v);// 设置freedrive 未定义
    void setFlag(unsigned int n, bool b);
    void setDigitalOut(unsigned int n, bool b);
    void setAnalogOut(unsigned int n, double f);
    bool setPayload(double m);
    void shutDown();
    void GetAllClientConnectedStatus(bool &dashboard, bool &primary, bool &secondary, bool &realtime);
    /*!
    * \brief 异步执行
    */
    void StopGently();
    /*!
    * \brief 执行dahsboard控制命令
    *
    * \param[in] 控制命令类型
    *
    * \returns 执行成功返回正确的回执消息
    */
    int DashboardControl(DashBoardCommand command);
    /*!
   * \brief 获取robot mode(可能睡眠)
   *
   * \returns 成功返回true
   */
    std::string GetRobotMode() const;

    /**
        @brief rtde停止脚本
    */
    void StopScript();

    /**
        @brief rtde停止脚本中servo线程
    */
    void StopJ();


private:
    bool WaitForProgramRunning();
    void TryWaitForUrRunning();
    void ResetSafetyError(int type);
    bool FilterSafetyRobotMode(int cycleCount);// 在一段时间内是否为正常模式
private:
    std::atomic_bool resetError_{false};
};

}// namespace CURCONTROL
#endif // CURCONTROL_H
